
/*======================== INCLUDES ===============================*/
#include "schm.h"
/*========================  GLOBALS ===============================*/
/* Space to Allocate the Task of Time triggers*/
OS_TaskConfigType Tasks[MAX_TIME_TASKS];

uint16_t time_counter;

OS_TaskConfigType * Running_Task;

OS_TaskStateType Actual_task;

SchM_BackGroundFlagType SchM_BackGroundFlag;

OS_StatusType OS_Status;
/*================== PRIVATE_FUNCTIONS_DEF ========================*/

/*=======================  FUNCTIONS ==============================*/

void SchM_Init(OS_TaskConfigType * SchMConf){
	uint8_t u8counter;
	uint8_t u8queuecounter;
	/* Init Gpt for PIT functionality to callback the OSTICK */
	Gpt_Init(NULL);
	/* Set the background flag */
	SchM_BackGroundFlag = FLAG_SET;
	/* Set Callback function with the API of gpt*/
	Gpt_SetChannelCallback(GPT_PIT_CHANNEL_0 , SchM_OsTick);
	if(SchMConf == NULL){
	  	Tasks[TASK_1MS] = OS_TaskConfig[TASK_1MS];
	  	Tasks[TASK_4MS] = OS_TaskConfig[TASK_4MS];
	  	Tasks[TASK_8MS] = OS_TaskConfig[TASK_8MS];
		Tasks[TASK_16MS] = OS_TaskConfig[TASK_16MS];
		Tasks[TASK_32MS] = OS_TaskConfig[TASK_32MS];
    	Tasks[TASK_64MS] = OS_TaskConfig[TASK_64MS];
	} 
	/* Init the time counter*/
	time_counter = 0;
	/* Reserve space for the task control block */
	OS_TaskControlBlock = (OS_TaskControlBlockType  *)MemAlloc_NearReserve(sizeof(OS_TaskControlBlockType) * MAX_TIME_TASKS);
	/* Initialize the task control block */
	for(u8counter = 0; u8counter<MAX_TIME_TASKS; u8counter++){
		/* setting the id */
		OS_TaskControlBlock[u8counter].TaskID = Tasks[u8counter].Task_ID;
		/* Getting the priority */
		OS_TaskControlBlock[u8counter].Priority = Tasks[u8counter].Priority;
		/* TODO configure stack */
		/* Setting the Deadlines */
		OS_TaskControlBlock[u8counter].Rel_Deadline = Tasks[u8counter].RelativeDeadline;
		/*Setting state to initlial state of SUSPENDED */
		OS_TaskControlBlock[u8counter].State = SUSPENDED;
	}
	/* Initialize the Queue */
	OS_Queue = (OS_QueueType *  )MemAlloc_NearReserve(sizeof(OS_QueueType) * OS_MAX_PRIORITY);
	for(u8counter=0;u8counter<OS_MAX_PRIORITY;u8counter++)
	{
		OS_Queue[u8counter].Queue = (OS_TaskType *)MemAlloc_NearReserve(sizeof(OS_TaskType) * OS_MAX_QUEUE_SIZE);
		/* Set initial values to Queue */
		for(u8queuecounter=0;u8queuecounter<OS_MAX_QUEUE_SIZE;u8queuecounter++)
		{
			OS_Queue[u8counter].Queue[u8queuecounter]= NONE;
		}
		OS_Queue[u8counter].u8QueueDepth = 0;
		OS_Queue[u8counter].u8index = 0;

	}
}

void SchM_DeInit(void){
	/* Stop the Timer for the Scheduler stop working */
	Gpt_StopTimer(GPT_PIT_CHANNEL_0);
}

void SchM_Start(void){
	/* Start the timer */
	Gpt_StartTimer(GPT_PIT_CHANNEL_0,0);
}

void SchM_BackGround(void){
	/* Call the Dispatcher */
	SchM_Dispatcher();
	/* Do background until an OS_tick setup a flag*/
	while(SchM_BackGroundFlag){
		/* TODO background stuff*/
	}
}

void SchM_OsTick(void){
	uint8_t temp_counter;
	/* Clear background flag to get out of the loop */
	SchM_BackGroundFlag = FLAG_CLEAR;
	/* Increment  the counter */
	time_counter++;
	/* activate de tasks */
	for(temp_counter=0;temp_counter<MAX_TIME_TASKS;temp_counter++)
	{
		if(( time_counter & Tasks[temp_counter].Mask ) == Tasks[temp_counter].Offset )
		{
			OS_Status = ActivateTask(Tasks[temp_counter].Task_ID);
		}
	}
}

void SchM_Dispatcher(void){
	uint8_t icounter;
	uint8_t jcounter;
	/* check the priority queu and run tasks in the queue */
	for(icounter = 1;icounter<=OS_MAX_PRIORITY;icounter++)
	{
		for(jcounter=0;jcounter<OS_MAX_QUEUE_SIZE;jcounter++)
		{
			if(OS_Queue[OS_MAX_PRIORITY-icounter].Queue[jcounter] != NONE){
				ActualTaskID = OS_Queue[OS_MAX_PRIORITY-icounter].Queue[jcounter];
				OS_TaskControlBlock[ActualTaskID].State = RUNNING;
				Tasks[ActualTaskID].Callback();
			}
		}
	}
}
